%% MOD-DH参数
%定义连杆的D-H参数
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%建立机器人模型
%       theta  d        a        alpha     
L1=Link([0     d1       a1       alpha1     ],'modified');
L2=Link([0     d2       a2       alpha2     ],'modified');L2.offset = pi/2;
L3=Link([0     d3       a3       alpha3     ],'modified');
L4=Link([0     d4       a4       alpha4     ],'modified');
L5=Link([0     d5       a5       alpha5     ],'modified');
L6=Link([0     d6       a6       alpha6     ],'modified');
%限制机器人的关节空间
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [(-95/180)*pi, (70/180)*pi];
L3.qlim = [(-85/180)*pi, (95/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];
%连接连杆，机器人取名为myrobot
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','myrobot');
robot.plot([0,0,0,0,0,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();%打印出机器人D-H参数表
robot.teach;%展示机器人模型
hold on;

%% 机器人模型定义
% 使用提供的机器人建模代码定义机器人模型

%% 工作空间边界
% 扩大工作空间范围以适应机器人实际能达到的位置
workspace_limits = [-1500, 1500;   % x轴范围扩大
                   -1500, 1500;   % y轴范围扩大
                   0, 2000];      % z轴范围扩大到2米

%% 定义多个障碍物
obstacles = [
    500, 1000, 1000, 100;   % [x, y, z, radius] - 第一个障碍物向上移动到z=800
    300, 1200, 1200, 80;    % 第二个障碍物向上移动到z=700
    700, 400, 1700, 120;   % 第三个障碍物向上移动到z=900
];

%% 笛卡尔空间路径规划
% 首先验证起始位置是否可达
q_test = [0, 0, 0, 0, 0, 0];
robot.plot(q_test); % 显示初始位置
pause(1);

% 定义更大幅度的起始和结束位置
q_start = [0, -pi/4, pi/3, 0, pi/3, 0];           % 起始位置：机械臂抬起
q_end = [2*pi/3, -pi/6, pi/4, pi/2, -pi/4, pi/2]; % 终止位置：大幅度转向右侧

% 通过正运动学计算对应的位姿
start_pose = robot.fkine(q_start);
end_pose = robot.fkine(q_end);

fprintf('起始位置: [%.2f, %.2f, %.2f]\n', start_pose.t(1), start_pose.t(2), start_pose.t(3));
fprintf('结束位置: [%.2f, %.2f, %.2f]\n', end_pose.t(1), end_pose.t(2), end_pose.t(3));

% 验证起始和结束位置是否在工作空间内
if ~check_workspace(start_pose.t', workspace_limits)
    error('起始位置在工作空间之外');
end
if ~check_workspace(end_pose.t', workspace_limits)
    error('结束位置在工作空间之外');
end

% 使用RRT规划器生成路径
max_iterations = 10000;
start_config = q_start;
end_config = q_end;

fprintf('开始RRT路径规划...\n');
path = rrt_planner(start_config, end_config, robot, obstacles, workspace_limits, max_iterations);

if isempty(path)
    error('RRT规划器无法找到有效路径');
end

% 使用RRT生成的路径替代之前的路径
num_points = size(path, 1);
joint_path = path;
cartesian_path = zeros(num_points, 6);

% 计算笛卡尔空间路径
for i = 1:num_points
    pose = robot.fkine(joint_path(i,:));
    cartesian_path(i, 1:3) = pose.t';
    cartesian_path(i, 4:6) = tr2rpy(pose.R);
end

%% 改进的可视化
% 创建主窗口
figure('Name', '机械臂避障路径规划', 'Position', [100, 100, 1200, 600]);

% 3D路径可视化
subplot(1,2,1);
robot.plot(joint_path(1,:));
hold on;

% 绘制工作空间边界
plot3([workspace_limits(1,1), workspace_limits(1,2)], [0, 0], [0, 0], 'k--');
plot3([0, 0], [workspace_limits(2,1), workspace_limits(2,2)], [0, 0], 'k--');
plot3([0, 0], [0, 0], [workspace_limits(3,1), workspace_limits(3,2)], 'k--');

% 绘制所有障碍物
for i = 1:size(obstacles, 1)
    [X, Y, Z] = sphere(20);
    X = X * obstacles(i,4) + obstacles(i,1);
    Y = Y * obstacles(i,4) + obstacles(i,2);
    Z = Z * obstacles(i,4) + obstacles(i,3);
    surf(X, Y, Z, 'FaceColor', 'r', 'FaceAlpha', 0.3, 'EdgeColor', 'none');
end

% 绘制笛卡尔空间路径
plot3(cartesian_path(:,1), cartesian_path(:,2), cartesian_path(:,3), 'b.-', 'MarkerSize', 10);

% 绘制起点和终点
plot3(start_pose.t(1), start_pose.t(2), start_pose.t(3), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot3(end_pose.t(1), end_pose.t(2), end_pose.t(3), 'ro', 'MarkerSize', 10, 'LineWidth', 2);

title('机械臂避障路径规划（笛卡尔空间）');
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
grid on;
view(3);

% 绘制关节角度变化
subplot(1,2,2);
time = linspace(0, 1, size(joint_path, 1));
joint_names = {'Joint 1', 'Joint 2', 'Joint 3', 'Joint 4', 'Joint 5', 'Joint 6'};
for i = 1:6
    plot(time, joint_path(:,i), 'LineWidth', 2);
    hold on;
end
title('关节角度随时间的变化');
xlabel('归一化时间');
ylabel('关节角度 (rad)');
legend(joint_names);
grid on;

% 动画展示（新窗口）
figure('Name', '机械臂运动动画');
robot.plot(joint_path, 'trail', 'b-', ...
    'workspace', [workspace_limits(1,:) workspace_limits(2,:) workspace_limits(3,:)]);

% 添加障碍物到动画窗口
hold on;
for i = 1:size(obstacles, 1)
    [X, Y, Z] = sphere(20);
    X = X * obstacles(i,4) + obstacles(i,1);
    Y = Y * obstacles(i,4) + obstacles(i,2);
    Z = Z * obstacles(i,4) + obstacles(i,3);
    surf(X, Y, Z, 'FaceColor', 'r', 'FaceAlpha', 0.3, 'EdgeColor', 'none');
end
hold off;

%% 速度级运动学分析
% 使用更好的测试构型
q_test = [0, -pi/6, pi/6, 0, pi/6, 0];  % 避开奇异位置
J = robot.jacob0(q_test);

% 打印雅可比矩阵分析结果
fprintf('\n===== 雅可比矩阵分析结果 =====\n');

% 打印雅可比矩阵
fprintf('\n雅可比矩阵:\n');
disp(J);

% 打印位置雅可比行列式
fprintf('\n位置雅可比行列式: %.4f\n', det(J(1:3,1:3)));

% 打印位置雅可比条件数
fprintf('位置雅可比条件数: %.4f\n', cond(J(1:3,1:3)));

% 打印关节速度
fprintf('\n关节速度:\n');
q_dot = pinv(J) * [0.1; 0; 0; 0; 0; 0];
for i = 1:length(q_dot)
    fprintf('Joint %d: %.6f\n', i, q_dot(i));
end

% 打印操作度
fprintf('\n操作度分析:\n');
w_pos = sqrt(det(J(1:3,1:3) * J(1:3,1:3)'));
w_ori = sqrt(det(J(4:6,4:6) * J(4:6,4:6)'));
fprintf('位置操作度: %.4f\n', w_pos);
fprintf('姿态操作度: %.4f\n', w_ori);

%% 分析路径上的操作度变化
path_manipulability = zeros(size(joint_path, 1), 2);
for i = 1:size(joint_path, 1)
    J = robot.jacob0(joint_path(i,:));
    path_manipulability(i,1) = sqrt(det(J(1:3,1:3) * J(1:3,1:3)'));  % 位置操作度
    path_manipulability(i,2) = sqrt(det(J(4:6,4:6) * J(4:6,4:6)'));  % 姿态操作度
end

figure;
plot(path_manipulability);
legend('位置操作度', '姿态操作度');
title('路径操作度分析');
xlabel('路径点');
ylabel('操作度');
grid on;

%% 辅助函数
function collision = check_collision(pos, obstacles)
    collision = false;
    for i = 1:size(obstacles, 1)
        if norm(pos - obstacles(i,1:3)) < obstacles(i,4)
            collision = true;
            return;
        end
    end
end

function in_workspace = check_workspace(pos, workspace_limits)
    in_workspace = all(pos >= workspace_limits(:,1)' & pos <= workspace_limits(:,2)');
end

%% RRT规划器及其辅助函数
function path = rrt_planner(start_config, end_config, robot, obstacles, workspace_limits, max_iterations)
    tree = start_config;
    parent = 1;  % 添加父节点索引跟踪
    path = [];
    
    fprintf('RRT规划开始，最大迭代次数: %d\n', max_iterations);
    
    for i = 1:max_iterations
        % 随机采样构型
        if rand < 0.1
            q_rand = end_config;
        else
            q_rand = random_config(robot);
        end
        
        % 找到最近的节点
        [~, idx] = min(sum((tree - q_rand).^2, 2));
        q_near = tree(idx,:);
        
        % 向随机构型方向扩展
        step_size = 0.1;
        direction = (q_rand - q_near) / norm(q_rand - q_near);
        q_new = q_near + step_size * direction;
        
        % 检查新节点是否有效
        if is_valid_config(q_new, robot, obstacles, workspace_limits)
            tree = [tree; q_new];
            parent = [parent; idx];  % 记录父节点
            
            % 检查是否到达目标
            if norm(q_new - end_config) < 0.1
                fprintf('找到路径！迭代次数: %d\n', i);
                path = reconstruct_path(tree, parent, size(tree,1));
                return;
            end
        end
        
        % 每100次迭代显示进度
        if mod(i, 100) == 0
            fprintf('已完成 %d 次迭代...\n', i);
        end
    end
    fprintf('达到最大迭代次数，未找到路径\n');
end

function q = random_config(robot)
    % 在关节限制范围内随机采样
    q = zeros(1,6);
    for i = 1:6
        q(i) = robot.links(i).qlim(1) + ...
               rand * (robot.links(i).qlim(2) - robot.links(i).qlim(1));
    end
end

function valid = is_valid_config(q, robot, obstacles, workspace_limits)
    % 检查关节角度是否在限制范围内
    for i = 1:6
        if q(i) < robot.links(i).qlim(1) || q(i) > robot.links(i).qlim(2)
            valid = false;
            return;
        end
    end
    
    % 检查末端执行器位置
    T = robot.fkine(q);
    pos = T.t';
    
    % 检查工作空间和碰撞
    valid = check_workspace(pos, workspace_limits) && ~check_collision(pos, obstacles);
end

function path = reconstruct_path(tree, parent, goal_idx)
    path = tree(goal_idx,:);
    current_idx = goal_idx;
    
    while current_idx > 1
        current_idx = parent(current_idx);
        path = [tree(current_idx,:); path];
    end
    
    % 路径平滑
    path = smooth_path(path);
end

function smoothed_path = smooth_path(path)
    % 简单的路径平滑处理
    window_size = 3;
    smoothed_path = path;
    
    for i = window_size:size(path,1)-window_size+1
        smoothed_path(i,:) = mean(path(i-window_size+1:i+window_size-1,:));
    end
end

